#include "sv2_video_base.h"
#include <opencv2/aruco.hpp>
#include <iostream>
#include <chrono>
#include <ctime>

#define SV2_MAX_FRAMES 52000
typedef unsigned char byte;

namespace sv2
{
  std::string _get_time_str(nlohmann::json tgts_timestamp)
  {
    long long milliseconds = static_cast<long long>(tgts_timestamp.get<double>() * 1e3);
    // 将毫秒数转换为std::chrono::milliseconds
    auto millisDuration = std::chrono::milliseconds(milliseconds);
    // 转换为system_clock::time_point
    auto timePoint = std::chrono::system_clock::time_point(millisDuration);
    // 转换为time_t
    std::time_t time = std::chrono::system_clock::to_time_t(timePoint);
    // 转换为本地时间的tm结构体
    std::tm t = *std::localtime(&time);

    //  计算毫秒部分
    auto totalMillis = std::chrono::duration_cast<std::chrono::milliseconds>(timePoint.time_since_epoch()).count();
    auto secondsPart = std::chrono::duration_cast<std::chrono::seconds>(timePoint.time_since_epoch()).count();
    auto millis = totalMillis - secondsPart * 1000;

    char buf[128];
    sprintf(buf, "%4d-%02d-%02d_%02d-%02d-%02d_%03ld", t.tm_year + 1900, t.tm_mon + 1, t.tm_mday, t.tm_hour, t.tm_min, t.tm_sec, millis);

    return std::string(buf);
  }

  nlohmann::json _spireview_format_json(nlohmann::json tgts_json)
  {
    nlohmann::json msg;
    nlohmann::json msg_2d = tgts_json["spirecv_msgs::2DTargets"];

    msg["w"] = msg_2d["width"];
    msg["h"] = msg_2d["height"];
    msg["fov"] = {msg_2d["fov_x"], msg_2d["fov_y"]};

    msg["fps"] = msg_2d["fps"];
    msg["fid"] = tgts_json["fid"];

    msg["rois"] = msg_2d["rois"];
    msg["time"] = _get_time_str(tgts_json["timestamp"]);

    msg["tgts"] = nlohmann::json::array();
    for (int i = 0; (int)i < (int)msg_2d["targets"].size(); i++)
    {
      nlohmann::json target;

      if (msg_2d["targets"][i].contains("tracked_id"))
      {
        target["tid"] = msg_2d["targets"][i]["tracked_id"];
      }
      else
      {
        target["sc"] = msg_2d["targets"][i]["score"];
        target["cat"] = msg_2d["targets"][i]["category_name"];
      }

      target["box"] = msg_2d["targets"][i]["bbox"];
      msg["tgts"].push_back(target);
    }

    return msg;
  }

  cv::Ptr<cv::aruco::Dictionary> _g_dict = nullptr;

  cv::Mat &_attach_aruco(int id, cv::Mat &img)
  {
    cv::Mat marker_img;
    std::vector<cv::Mat> ch(3);
    cv::aruco::Dictionary dict = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_5X5_1000);
    ch[0] = cv::Mat::zeros(22, 22, CV_8UC1);
    ch[1] = cv::Mat::zeros(22, 22, CV_8UC1);
    ch[2] = cv::Mat::zeros(22, 22, CV_8UC1);

    ch[0].setTo(cv::Scalar(255));
    ch[1].setTo(cv::Scalar(255));
    ch[2].setTo(cv::Scalar(255));
    cv::Rect inner_roi = cv::Rect(4, 4, 14, 14);
    cv::Rect full_roi = cv::Rect(img.cols - 22, img.rows - 22, 22, 22);

    int id_k = id % 1000;

    // dict.drawMarker(id_k, 14, marker_img, 1);
    cv::aruco::generateImageMarker(dict, id_k, 14, marker_img, 1);
    marker_img.copyTo(ch[0](inner_roi));
    // dict.drawMarker(id_k, 14, marker_img, 1);
    cv::aruco::generateImageMarker(dict, id_k, 14, marker_img, 1);
    marker_img.copyTo(ch[1](inner_roi));
    // dict.drawMarker(id_k, 14, marker_img, 1);
    cv::aruco::generateImageMarker(dict, id_k, 14, marker_img, 1);
    marker_img.copyTo(ch[2](inner_roi));

    cv::merge(ch, marker_img);
    marker_img.copyTo(img(full_roi));
    return img;
  }

  int _parse_aruco(cv::Mat &img)
  {
    int id;
    cv::Mat marker_img;
    std::vector<cv::Mat> ch(3);
    if (_g_dict == nullptr)
    {
      _g_dict = new cv::aruco::Dictionary;
      *_g_dict = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_5X5_1000);
    }
    cv::Rect full_roi = cv::Rect(img.cols - 22, img.rows - 22, 22, 22);
    img(full_roi).copyTo(marker_img);
    cv::split(marker_img, ch);

    std::vector<int> id_i;
    std::vector<int> id_k;
    std::vector<int> id_m;
    std::vector<std::vector<cv::Point2f>> marker_corners;
    cv::aruco::detectMarkers(ch[0], _g_dict, marker_corners, id_i);
    cv::aruco::detectMarkers(ch[1], _g_dict, marker_corners, id_k);
    cv::aruco::detectMarkers(ch[2], _g_dict, marker_corners, id_m);
    if (id_i.size() > 0 || id_k.size() > 0 || id_m.size() > 0)
    {
      if (id_i.size() > 0)
        id = id_i[0];
      else if (id_k.size() > 0)
        id = id_k[0];
      else if (id_m.size() > 0)
        id = id_m[0];
    }
    else
    {
      // std::cout << "error ch0 & ch1" << std::endl;
      id = -1;
    }

    return id;
  }

  VideoWriterBase::VideoWriterBase()
  {
    this->_is_running = false;
    this->_fid = 0;
    this->_fcnt = 0;
  }
  VideoWriterBase::~VideoWriterBase()
  {
    this->release();
    // this->_tt.join();
  }
  cv::Size VideoWriterBase::getSize()
  {
    return this->_image_size;
  }
  double VideoWriterBase::getFps()
  {
    return this->_fps;
  }
  std::string VideoWriterBase::getFilePath()
  {
    return this->_file_path;
  }
  bool VideoWriterBase::isRunning()
  {
    return this->_is_running;
  }
  void VideoWriterBase::setup(std::string file_path, cv::Size size, double fps, bool with_targets)
  {
    this->_file_path = file_path;
    this->_fps = fps;
    this->_image_size = size;
    this->_with_targets = with_targets;

    this->_init();

    this->_tt = std::thread(&VideoWriterBase::_run, this);
    this->_tt.detach();
  }
  void VideoWriterBase::write(cv::Mat image, nlohmann::json tgts_json)
  {
    if (this->_is_running)
    {
      cv::Mat image_put;
      if (this->_image_size.height == image.rows && this->_image_size.width == image.cols)
      {
        image.copyTo(image_put);
      }
      else
      {
        char msg[256];
        sprintf(msg, "SpireCV 2.0 (106) Input image SIZE (%d, %d) != Saving SIZE (%d, %d)!", image.cols, image.rows, this->_image_size.width, this->_image_size.height);
        throw std::runtime_error(msg);
        // cv::resize(image, image_put, this->_image_size);
      }

      if (this->_targets_ofs)
      {
        this->_fid++;
        image_put = _attach_aruco(this->_fid, image_put);
        tgts_json["fid"] = this->_fid;
        nlohmann::json json_str = _spireview_format_json(tgts_json);
        this->_tgts_to_write.push(json_str);
        if (this->_fid >= SV2_MAX_FRAMES)
          this->_fid = 0;
      }
      this->_image_to_write.push(image_put);
    }
  }
  void VideoWriterBase::_run()
  {
    while (this->_is_running && isOpenedImpl())
    {
      while (!this->_image_to_write.empty())
      {
        this->_fcnt++;

        cv::Mat img = _image_to_write.front();
        if (this->_targets_ofs)
        {
          if (!this->_tgts_to_write.empty())
          {
            nlohmann::json json_msg = this->_tgts_to_write.front();

            _targets_ofs << json_msg << std::endl;

            this->_tgts_to_write.pop();
          }
        }
        // this->_writer << img;
        writeImpl(img);
        this->_image_to_write.pop();

        if (this->_fcnt >= SV2_MAX_FRAMES)
        {
          _init();
        }
      }
      std::this_thread::sleep_for(std::chrono::milliseconds(int(1000 / this->_fps)));
    }
  }

  void VideoWriterBase::_init()
  {
    this->release();

    // get now time
    time_t t = time(NULL);
    ;
    tm *local = localtime(&t);

    char s_buf[128];
    strftime(s_buf, 64, "/FlyVideo_%Y-%m-%d_%H-%M-%S", local);
    std::string name = std::string(s_buf);

    bool opend = false;
    opend = setupImpl(name);

    if (!opend)
    {
      std::cout << "Failed to write video: " << _file_path + name << std::endl;
    }
    else
    {
      this->_is_running = true;
      if (this->_with_targets)
      {
        this->_targets_ofs.open(this->_file_path + name + ".svj");
        if (!this->_targets_ofs)
        {
          std::cout << "Failed to write info file: " << this->_file_path << std::endl;
          this->_is_running = false;
        }
      }
    }
  }
  void VideoWriterBase::release()
  {
    this->_is_running = false;
    this->_fid = 0;
    this->_fcnt = 0;

    if (this->_targets_ofs.is_open())
      this->_targets_ofs.close();

    while (!this->_image_to_write.empty())
      this->_image_to_write.pop();
    while (!this->_tgts_to_write.empty())
      this->_tgts_to_write.pop();

    releaseImpl();
  }
  bool VideoWriterBase::setupImpl(std::string file_name_)
  {
    return false;
  }
  bool VideoWriterBase::isOpenedImpl()
  {
    return false;
  }
  void VideoWriterBase::writeImpl(cv::Mat img_)
  {
  }
  void VideoWriterBase::releaseImpl()
  {
  }

}
